#include <mc_controller/stanley.h>

Stanley::Stanley(){
    kpLinear_ = 0.5;
}

Stanley::~Stanley(){

}


// 计算局部目标点的速度
mc_status Stanley::computeTwist(geometry_msgs::PoseStamped local_goal, geometry_msgs::Twist& cmd_vel){
    mc_status status;
    double goal_x = local_goal.pose.position.x;
    double goal_y = local_goal.pose.position.y;

    // 前瞻距离
    double lookhead_distance = hypot(goal_x, goal_y);
    // 横向误差（假设机器人本地坐标的路径点）
    double lateral_error = goal_y;
    // 航向误差（机器人朝向与目标点方向的角度差）
    double heading_error = math_utils::diffAngle(0.0, atan2(goal_y, goal_x));
    // 转向角
    double steering_angle = heading_error + std::atan2(lateral_error, lookhead_distance);
    cout << "[Stanley] steering_angle: " << steering_angle 
        << ", lateral_error: " << lateral_error << endl;

    if(isPositionReached(localOriginPose_, local_goal)){
        if(!isAngleReached(localOriginPose_, local_goal)){
            rotateToTGoal(localOriginPose_, local_goal, cmd_vel);
        }
    } 
    else{
        // 距离越远，速度越快
        cmd_vel.linear.x = kpLinear_ * lookhead_distance;  // 简单的PID控制
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = cmd_vel.linear.x * tan(steering_angle);
    }    

    return status;
}